#conda install --channel https://conda.anaconda.org/menpo opencv   ##使用conda安装cv2库

import cv2
import numpy as np
import glob
import pickle

# 准备棋盘图参数
chessboard_size = (9, 6)
criteria = (cv2.TermCriteria_EPS + cv2.TermCriteria_MAX_ITER, 30, 0.001)

# 获取棋盘角点的世界坐标
objp = np.zeros((chessboard_size[0] * chessboard_size[1], 3), np.float32)
#这个数组用于存储棋盘角点的世界坐标，每个角点有三个坐标值（x, y, z）。
objp[:, :2] = np.mgrid[0:chessboard_size[0], 0:chessboard_size[1]].T.reshape(-1, 2)
objp *= 21.08  # 将单位从像素转换为毫米，由棋盘格的实际尺寸决定

#获取棋盘格角点的世界坐标矩阵
# print(objp)

# 存储棋盘角点
objpoints = []  # 3D 世界坐标
imgpoints_left = []  # 左相机 2D 图像坐标
imgpoints_right = []  # 右相机 2D 图像坐标

# 加载两部手机的棋盘图片
images_left = glob.glob(r'E:\University\Junior_up\twelfth_xing_huo_bei\Volume_Measure\Using Python\Photograph_Data\Left_view\*.jpg')
images_right = glob.glob(r'E:\University\Junior_up\twelfth_xing_huo_bei\Volume_Measure\Using Python\Photograph_Data\Right_view\*.jpg')

#检测图片加载是否正常
# print(f"Left images count: {len(images_left)}")
# print(f"Right images count: {len(images_right)}")

# 处理左相机图像
for fname in images_left:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)        #将彩色图像转换为灰度图像，以便后续处理。

    # cv2.imshow('img with corners', gray)
    # cv2.waitKey(0)  # 等待按键，然后关闭窗口

    ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)

    #Verify that corners are detected for each image by printing the ret value
    # print(f"Left image {fname}, Corners found: {ret}")

    if ret:
        objpoints.append(objp)                               # 将棋盘角点的世界坐标添加到objpoints列表中。
        corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) #cornerSubPix 函数对检测到的角点进行亚像素级别的精确化。criteria 是一个包含迭代次数和精度要求的结构体。
        imgpoints_left.append(corners2)

# 单独标定左相机
ret_left, mtx_left, dist_left, rvecs_left, tvecs_left = cv2.calibrateCamera(objpoints, imgpoints_left, gray.shape[::-1], None, None)

# 处理右相机图像（类似左相机）
for fname in images_right:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)
    if ret:
        corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
        imgpoints_right.append(corners2)

# 单独标定右相机
ret_right, mtx_right, dist_right, rvecs_right, tvecs_right = cv2.calibrateCamera(objpoints, imgpoints_right, gray.shape[::-1], None, None)

# 标定双目相机
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate(
    objpoints, imgpoints_left, imgpoints_right, mtx_left, dist_left, mtx_right, dist_right, gray.shape[::-1], criteria=criteria, flags=cv2.CALIB_FIX_INTRINSIC
)

# 在此步骤中，得到的参数包括：

# cameraMatrix1 和 cameraMatrix2：左右相机的内参矩阵。
# distCoeffs1 和 distCoeffs2：左右相机的畸变系数。
# R：左相机到右相机的旋转矩阵。
# T：左相机到右相机的平移向量。

print(cameraMatrix1)
print(cameraMatrix2)
print(distCoeffs1)
print(distCoeffs2)
print(R)
print(T)

# 立体校正
# R1和R2分别表示左目相机和右目相机在新的坐标系下的旋转矩阵
# P1和P2分别表示左目相机和右目相机在新的坐标系下的投影矩阵
# Q：表示重投影矩阵，用于计算视差图
R1, R2, P1, P2, Q, _, _ = cv2.stereoRectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, gray.shape[::-1], R, T)

# Save the parameters
calibration_data = {
    'cameraMatrix1': cameraMatrix1,
    'distCoeffs1': distCoeffs1,
    'cameraMatrix2': cameraMatrix2,
    'distCoeffs2': distCoeffs2,
    'R': R,
    'T': T,
    'E': E,
    'F': F,
    'R1': R1,
    'R2': R2,
    'P1': P1,
    'P2': P2,
    'Q': Q
}

# Save calibration data using pickle
with open(r'E:\University\Junior_up\twelfth_xing_huo_bei\Volume_Measure\Using Python\Photograph_Data\stereo_calibration_data.pkl', 'wb') as f:
    pickle.dump(calibration_data, f)
print("Calibration data saved successfully.")

# Load the calibration data when needed
# def load_calibration_data(file_path='stereo_calibration_data.pkl'):
#     with open(file_path, 'rb') as f:
#         data = pickle.load(f)
#     return data

# Example usage: Load and access calibration data
# calibration_data_loaded = load_calibration_data()
# print("Loaded calibration data:", calibration_data_loaded)